#include "GipsyModule.h"
#include "DSP2833x_Device.h"
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

bool GipsyModule::Initialize()
{
	if( Init() == false ) return false;
	
	// switch to 38k
	SendData("$PUBX,41,1,0007,0003,38400,0*20\r\n"); 
	SendData("$PUBX,41,1,0007,0003,38400,0*20\r\n"); // resend
	
	// switch port to 38k
	ScicRegs.SCICTL1.all =0x0003;  // enable TX, RX, internal SCICLK, Disable RX ERR, SLEEP, TXWAKE
	ScicRegs.SCIHBAUD    =0x0000;  // switch to 0x79 for 38400
	ScicRegs.SCILBAUD    =0x0079;
	ScicRegs.SCICTL1.all =0x0023;  // Relinquish SCI from Reset  
	
	// reset values
	RXBufPosition = 0;
	m_time[0] = 0;
	m_latitude[0] = 0;
	m_longitude[0] = 0;
	m_fixStatus = 0;
	m_satUsed = 0;
	m_alt = 0;
	m_NS = 'S';
	m_EW = 'W';
	m_Lat = 0;
	m_Long = 0;
	
	return true;
}

void GipsyModule::GetCoordinates(char* buffer)
{
	sprintf(buffer, "Lat: %s%c, Long: %s%c, Alt: %f", m_latitude, m_NS, m_longitude, m_EW, m_alt);	
}

void GipsyModule::GetFixInfo(char* buffer)
{
	sprintf(buffer, "Status: %d, Satellites: %d, Time: %s", m_fixStatus, m_satUsed, m_time);
}

void GipsyModule::GetTime(char* buffer)
{
	strcpy(buffer, m_time);
}

double GipsyModule::GetLatitude()
{
	return m_Lat;
}

double GipsyModule::GetLongitude()
{
	return m_Long;
}

float GipsyModule::GetAltitude()
{
	return m_alt;
}

int GipsyModule::GetSatUsed()
{
	return m_satUsed;
}
									
void GipsyModule::Update()
{
	char buff[500]; // must be called at max rate of 10Hz! (100ms) (4800bytes/second)
	Uint32 len = GetData(buff);

	// parse data
	if( len > 0 )
	{
		buff[len] = 0;
		ParseCommand(buff);
	}		
}

int GipsyModule::ParseCommand(char* input) // returns message length
{
	for(int i=0; i!= strlen(input); i++)
	{
		char data = input[i];
		if( data == '\r' )
		{
			// end of command
			int size = RXBufPosition;
			RXBufPosition = 0;
			RXBuffer[size] = 0; // null terminate

			if( CheckChecksum(RXBuffer) == true )
			{			
				ExecuteCommand(RXBuffer, size);
			}
		}
		else
		{
			if( RXBufPosition >= 200 || data == '$')
			{
				// overrun or RESET
				RXBufPosition = 0;
			}
			RXBuffer[RXBufPosition] = data;
			RXBufPosition++;
		}
	}
	return 0;
}

void GipsyModule::ExecuteCommand(char* input, int size)
{
	// parse command
	char* token = strtok( input, ",");
	if( !strcmp(token, "$GPGGA") )
	{		
		token = strtok(NULL, ",");
		if( token != NULL )
		{
			// UTC time
			strcpy(m_time, token);
		}
		token = strtok(NULL, ",");
		if( token != NULL )
		{
			// Latitude
			strcpy(m_latitude, token);

			// convert lat
			double num = atof(m_latitude);
			int deg = (int)floor(num/100);
			double min = num-deg*100;
			double total = deg + min/60;
			m_Lat = total;
		} 
		token = strtok(NULL, ",");
		if( token != NULL )
		{
			// N/S
			m_NS = token[0];
		}
		token = strtok(NULL, ",");
		if( token != NULL )
		{
			// Longitude
			strcpy(m_longitude, token);

			// convert lon
			double num = atof(m_longitude);
			int deg = (int)floor(num/100);
			double min = num-deg*100;
			double total = deg + min/60;
			m_Long = total;
		}
		token = strtok(NULL, ",");
		if( token != NULL )
		{
			// E/W
			m_EW = token[0];
		}
		token = strtok(NULL, ",");
		if( token != NULL )
		{
			// Position Fix
			m_fixStatus = atoi(token);
			
			if( m_fixStatus == 0 ) m_satUsed = 0; // no fix
		}
		token = strtok(NULL, ",");
		if( token != NULL )
		{
			if( token[0] == '0' ) token++;

			// Sat Used
			m_satUsed = atoi(token);
		}
		else
		{
			m_satUsed = 0; // no fix/errors
		}
		token = strtok(NULL, ",");
		if( token != NULL )
		{
			// HDOP
		}
		token = strtok(NULL, ",");
		if( token != NULL )
		{
			// msl
			m_alt = atof(token);
		}
	}
}

unsigned char GipsyModule::CalculateChecksum(char* str, int len)
{
	unsigned char checksum = 0;

	for(int i=0; i!= len; i++ )
	{
		checksum = checksum^str[i];
	}

	return checksum;
}

bool GipsyModule::CheckChecksum(char* str)
{
	int len = strlen(str);
	if(str[0] != '$') return false; // invalid start character
	
	for(int starpos=0; starpos!=len-2; starpos++ )
	{
		if(str[starpos] == '*' )
		{
			unsigned char calcChecksum = CalculateChecksum(&str[1], starpos-1);
			unsigned int checkSum;
			sscanf(&str[starpos+1], "%x", &checkSum);
			bool valid = (checkSum == calcChecksum);

			return valid;
		}
	}

	return false;
}
